
#ifdef WIN32
#include <windows.h>
#endif
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdint.h>
#include <iostream>
#include <slvs.h>

static Slvs_System sys;

static void *CheckMalloc(size_t n)
{
    void *r = malloc(n);
    if (!r)
    {
        printf("out of memory!\n");
        exit(-1);
    }
    return r;
}

/*-----------------------------------------------------------------------------
 * An example of a constraint in 3d. We create a single group, with some
 * entities and constraints.
 *---------------------------------------------------------------------------*/
void Example3d()
{
    /* This will contain a single group, which will arbitrarily number 1. */
    Slvs_hGroup g = 1;

    /* A point, initially at (x y z) = (10 10 10) */
    sys.addPara(Slvs_MakeParam(1, g, 10.0));
    sys.addPara(Slvs_MakeParam(2, g, 20.0));
    sys.addPara(Slvs_MakeParam(3, g, 0.30));
    sys.addEntity(Slvs_MakePoint3d(101, g, 1, 2, 3));
    /* and a second point at (20 20 20) */
    sys.addPara(Slvs_MakeParam(4, g, 10.0));
    sys.addPara(Slvs_MakeParam(5, g, 2.230));
    sys.addPara(Slvs_MakeParam(6, g, 0.1230));
    sys.addEntity(Slvs_MakePoint3d(102, g, 4, 5, 6));

    // sys.addPara(Slvs_MakeParam(7, g, 0.0));
    // sys.addPara(Slvs_MakeParam(8, g, 0.0));
    // sys.addPara(Slvs_MakeParam(9, g, 2.0));
    // sys.addEntity(Slvs_MakePoint3d(103, g, 7, 8, 9));

    /* and a line segment connecting them. */
    // sys.addEntity(Slvs_MakeLineSegment(200, g,
    //                                     SLVS_FREE_IN_3D, 101, 102));

    /* The distance between the points should be 30.0 units. */
    // sys.constraint[sys.constraints++] = Slvs_MakeConstraint(
    //                                         1, g,
    //                                         SLVS_C_PT_PT_DISTANCE,
    //                                         SLVS_FREE_IN_3D,
    //                                         30.0,
    //                                         101, 102, 0, 0);

    // sys.entity[sys.entities++] = Slvs_MakeWorkplane(300, g, 101, 102);

    // The distance from point ptA to line segment entityA is equal to valA.
    /* Let's tell the solver to keep the second point as close to constant
     * as possible, instead moving the first point. */
    // sys.dragged[0] = 1;
    // sys.dragged[1] = 2;
    // sys.dragged[2] = 3;
    // sys.dragged[3] = 4;
    // sys.dragged[4] = 5;
    // sys.dragged[5] = 6;

    sys.constraint[sys.constraints++] = Slvs_MakeConstraint(
        1, g,
        SLVS_C_WHERE_DRAGGED,
        SLVS_FREE_IN_3D,
        0.0,
        101, 0, 0, 0);

    sys.constraint[sys.constraints++] = Slvs_MakeConstraint(
        2, g,
        SLVS_C_PT_PT_DISTANCE,
        SLVS_FREE_IN_3D,
        523.11230,
        101, 102, 0, 0);

    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            std::cout << sys.param[i * 3 + j].val << " ";
        }
        std::cout << std::endl;
    }

    /* Now that we have written our system, we solve. */
    Slvs_Solve(&sys, g);
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            std::cout << sys.param[i * 3 + j].val << " ";
        }
        std::cout << std::endl;
    }
    if (sys.result == SLVS_RESULT_OKAY)
    {
        printf("okay; now at p1(%lf %lf %lf)\n"
               "             p2(%lf %lf %lf),\n            p3(%lf %lf %lf)\n",
               sys.param[0].val, sys.param[1].val, sys.param[2].val,
               sys.param[3].val, sys.param[4].val, sys.param[5].val,
               sys.param[6].val, sys.param[7].val, sys.param[8].val);
        printf("%d DOF\n", sys.dof);
    }
    else
    {
        printf("solve failed %d\n", sys.result);
    }
}


int main()
{
    sys.param = new Slvs_Param[1024];           //*sizeof(sys.param[0]));
    sys.constraint = new Slvs_Constraint[1024]; // CheckMalloc(50*sizeof(sys.constraint[0]));
    sys.failed = new Slvs_hConstraint[1024];    // CheckMalloc(50*sizeof(sys.failed[0]));
    sys.faileds = 1024;

    Example3d();
    return 0;
}
